﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using FaceTrackerAMEE.Model;
using AMEE.PanTiltControl;
using System.Diagnostics;

namespace FaceTrackerAMEE
{
    /// <summary>
    /// Interaction logic for FaceTrackerView.xaml.<para>
    /// Class that uses the Face Tracking SDK to display a face mask for
    /// tracked skeletons</para>
    /// </summary>
    public partial class FaceTrackerView : UserControl, IDisposable
    {
        #region Fields

        /// <summary>True if this is disposed.</summary>
        bool _disposed;
        /// <summary>Kinect's rgb frame</summary>
        byte[] _colorImage;
        /// <summary>Kinect's rgb image formats (framerate, resolution,...)</summary>
        ColorImageFormat _colorImageFormat = ColorImageFormat.Undefined;
        /// <summary>Kinect's depth frame</summary>
        short[] _depthImage;
        /// <summary>Kinect's depth image formats (framerate, resolution,...)</summary>
        DepthImageFormat _depthImageFormat = DepthImageFormat.Undefined;
        /// <summary>The skeleton data read from kinect.</summary>
        Skeleton[] _skeletonData;
        /// <summary>100. Max number of Frames without face tracking, before Tracker will be removed.</summary>
        const uint _MAX_MissedFrames = 100;

        //// The Pan / Tilt radiants last sent.
        //float _lastPan;
        //float _lastTilt;

        static readonly PanTilt Hals = new PanTilt();

        /// <summary>The Face trackers for tracked skeletons; key=kinect's skelton Tracking ID value=SkeletonFaceTracker</summary>
        readonly Dictionary<int, SkeletonFaceTracker> _trackedSkeletons = new Dictionary<int, SkeletonFaceTracker>();

        #endregion

        #region Properties

        /// <summary>
        /// Gets or sets the kinect sensor.
        /// </summary>
        /// <value>
        /// The kinect.
        /// </value>
        /// <remarks>This is the wrapper of the wpf dependency property 'KinectPorpert'</remarks>
        public KinectSensor Kinect {
            get { return (KinectSensor)this.GetValue(KinectProperty); }
            set { this.SetValue(KinectProperty, value); }
        }

        /// <summary>
        /// The kinect (WPF dependency) property, named "Kinect".
        /// </summary>
        public static readonly DependencyProperty KinectProperty = DependencyProperty.Register(
            "Kinect",
            typeof(KinectSensor),
            typeof(FaceTrackerView),
            new PropertyMetadata(
                null, // default
                (o, args) => ((FaceTrackerView)o).OnSensorChanged((KinectSensor)args.OldValue, (KinectSensor)args.NewValue)) // propertyChangedCallback
                );

        #endregion


        #region Constructor(s)

        /// <summary>
        /// Initializes a new instance of the <see cref="FaceTrackerView"/> class.
        /// </summary>
        public FaceTrackerView() {
            InitializeComponent();
            FaceTrackerView.Hals.Move(0f, 0f);
        }
        /// <summary>
        /// Finalizes an instance of the <see cref="FaceTrackerView"/> class.
        /// </summary>
        ~FaceTrackerView() {
            this.Dispose(false);
        } 
        #endregion


        #region Public

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        /// <remarks>Implementing IDisposable.</remarks>
        public void Dispose() {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Called when the KinectSensor (KinectProperty) changes.
        /// </summary>
        /// <param name="oldSensor">The old sensor.</param>
        /// <param name="newSensor">The new sensor.</param>
        /// <remarks>
        /// Remove old trackers; Un-/register AllFramesReady Handler.
        /// </remarks>
        public void OnSensorChanged(KinectSensor oldSensor, KinectSensor newSensor) {
            if (oldSensor != null) {
                oldSensor.AllFramesReady -= this.OnAllFramesReady;
                this.resetFaceTracking();
            }
            if (newSensor != null) {
                newSensor.AllFramesReady += this.OnAllFramesReady;
            }
        }

        #endregion

        /// <summary>
        /// Drawing tracked Face point. Participates in rendering operations that are directed by the layout system.
        /// The rendering instructions for this element are not used directly when this method is invoked, and are instead preserved for later asynchronous use by layout and drawing.
        /// </summary>
        /// <param name="drawingContext">The drawing instructions for a specific element. This context is provided to the layout system.</param>
        protected override void OnRender(DrawingContext drawingContext) {
            base.OnRender(drawingContext);
            bool follow = true;
            foreach (var faceInformation in this._trackedSkeletons.Values) {
                faceInformation.DrawPointOfInterest(drawingContext);
                // Follow only if tracked and only first at list
                if (faceInformation.LastFaceTrackSucceeded) {
                    if (follow) {
                        var vec = faceInformation.Eye3D;
                        //this.Follow(-vec.X, -vec.Y, vec.Z);
                        this.MoveTo(-vec.X, -vec.Y, vec.Z);
                        follow = false;
                    }
                }
            }
            // after a while, lost faces will be removed. Go back to default position
            if (0 == _trackedSkeletons.Count) {
                this.MoveTo(0f, 0f, 0f);
            }
        }

        ///// <summary>
        ///// Für Kinect an PanTilt montiert. Kinect zu schwer!!
        ///// 
        ///// Move the PanTilt Device, so it is orientated to the tracked eye.<para>
        ///// The specified coords are the kinect one (and relative to the device's).
        ///// </para>
        ///// </summary>
        //public void Follow(float x, float y, float z) {
            
        //    float pan = (float)Math.Atan2(x, z) + FaceTrackerView.Hals.PositionRadPan;
        //    float tilt = (float)Math.Atan2(y, z) + FaceTrackerView.Hals.PositionRadTilt;
        //    Debug.WriteLine(String.Format("Follow - PanTilt.Move({0:F3} ; {1:F3}) = ({2:F2}° ; {3:F2}°)", pan, tilt, (180*pan/Math.PI), (180*tilt/Math.PI)), "Info FaceTrackerView");
        //    FaceTrackerView.Hals.Move(pan, tilt);
        //}

        /// <summary>
        /// Move the PanTilt Device to the specified orientation (in the device's coords).
        /// </summary>
        /// <param name="x">The x.</param>
        /// <param name="y">The y.</param>
        /// <param name="z">The z.</param>
        public void MoveTo(float x, float y, float z) {
            float pan = (float)Math.Atan2(x, z);
            float tilt = (float)Math.Atan2(y, z);
            Debug.WriteLine(String.Format("Follow - PanTilt.Move({0:F3} ; {1:F3}) = ({2:F2}° ; {3:F2}°)", pan, tilt, (180 * pan / Math.PI), (180 * tilt / Math.PI)), "Info FaceTrackerView");
            FaceTrackerView.Hals.Move(pan, tilt);
        }


        #region Private

        /// <summary>
        /// Handler of the kinect's event, fired when new frames are available.<para>
        /// Reads data, adds new FaceTracker for new Skeletons, does face tracking.
        /// </para>
        /// </summary>
        /// <param name="sender">The sender.</param>
        /// <param name="allFramesReadyEventArgs">The <see cref="AllFramesReadyEventArgs"/> instance containing the event data.</param>
        void OnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs) {
            ColorImageFrame colorImageFrame = null;
            DepthImageFrame depthImageFrame = null;
            SkeletonFrame skeletonFrame = null;

            try {
                // read frames
                colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame();
                depthImageFrame = allFramesReadyEventArgs.OpenDepthImageFrame();
                skeletonFrame = allFramesReadyEventArgs.OpenSkeletonFrame();
                if (colorImageFrame == null || depthImageFrame == null || skeletonFrame == null) {
                    return;
                }

                // Check for image format changes.  The FaceTracker doesn't
                // deal with that so we need to reset.
                if (_depthImageFormat != depthImageFrame.Format) {
                    //this.ResetFaceTracking();
                    _depthImage = null;
                    _depthImageFormat = depthImageFrame.Format;
                }
                if (_colorImageFormat != colorImageFrame.Format) {
                    //this.ResetFaceTracking();
                    _colorImage = null;
                    _colorImageFormat = colorImageFrame.Format;
                }

                // Create any buffers to store copies of the data we work with
                if (_depthImage == null) {
                    _depthImage = new short[depthImageFrame.PixelDataLength];
                }
                if (_colorImage == null) {
                    _colorImage = new byte[colorImageFrame.PixelDataLength];
                }
                // Get the skeleton information
                if (_skeletonData == null || _skeletonData.Length != skeletonFrame.SkeletonArrayLength) {
                    _skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];
                }
                colorImageFrame.CopyPixelDataTo(_colorImage);
                depthImageFrame.CopyPixelDataTo(_depthImage);
                skeletonFrame.CopySkeletonDataTo(_skeletonData);

                // Update the list of trackers and the trackers with the current frame information
                foreach (Skeleton skeleton in _skeletonData) {
                    // only for tracked skeletons
                    if (skeleton.TrackingState == SkeletonTrackingState.Tracked || skeleton.TrackingState == SkeletonTrackingState.PositionOnly) {
                        // add new FaceTracker for new Skeletons
                        if (!_trackedSkeletons.ContainsKey(skeleton.TrackingId)) {
                            _trackedSkeletons.Add(skeleton.TrackingId, new SkeletonFaceTracker() { });
                        }
                        // do face tracking
                        SkeletonFaceTracker skeletonFaceTracker;
                        if (_trackedSkeletons.TryGetValue(skeleton.TrackingId, out skeletonFaceTracker)) {
                            if (this.Kinect != null) {
                                skeletonFaceTracker.OnFrameReady(this.Kinect, _colorImageFormat, _colorImage, _depthImageFormat, _depthImage, skeleton);
                                skeletonFaceTracker.LastTrackedFrame = skeletonFrame.FrameNumber;
                            }
                        }
                    }

                }

                this.removeOldTrackers(skeletonFrame.FrameNumber);
                base.InvalidateVisual(); // rerender
            }
            finally {
                // after 'Open...Frame', values must be disposed
                if (colorImageFrame != null) {
                    colorImageFrame.Dispose();
                }
                if (depthImageFrame != null) {
                    depthImageFrame.Dispose();
                }
                if (skeletonFrame != null) {
                    skeletonFrame.Dispose();
                }
            }
        }

        /// <summary>
        /// Clear out any trackers for skeletons we haven't heard from for a while
        /// </summary>
        /// <param name="currentFrameNumber">The current frame number.</param>
        void removeOldTrackers(int currentFrameNumber) {
            var trackersToRemove = new List<int>();

            foreach (var tracker in _trackedSkeletons) {
                uint missedFrames = (uint)currentFrameNumber - (uint)tracker.Value.LastTrackedFrame;
                if (missedFrames > _MAX_MissedFrames) {
                    // There have been too many frames since we last saw this skeleton
                    trackersToRemove.Add(tracker.Key);
                }
            }
            foreach (int trackingId in trackersToRemove) {
                this.removeTracker(trackingId);
            }
        }
        /// <summary>
        /// Removes and disposes the corresponding face tracker.
        /// </summary>
        /// <param name="trackingId">The tracking id.</param>
        void removeTracker(int trackingId) {
            _trackedSkeletons[trackingId].Dispose();
            _trackedSkeletons.Remove(trackingId);
        }

        /// <summary>
        /// Resets the face tracking (remove and dispose all trackers).
        /// </summary>
        void resetFaceTracking() {
            foreach (int trackingId in new List<int>(_trackedSkeletons.Keys)) {
                this.removeTracker(trackingId);
            }
        }


        /// <summary>
        /// Releases unmanaged and - optionally - managed resources.
        /// </summary>
        /// <param name="disposing"><c>true</c> to release both managed and unmanaged resources; <c>false</c> to release only unmanaged resources.</param>
        protected virtual void Dispose(bool disposing) {
            if (!this._disposed) {
                this.resetFaceTracking();

                this._disposed = true;
            }
        }


        #endregion
    }
}
